#include "PenaltyKick.h"
#include "skill/Factory.h"
#include "TaskMediator.h"
#include "WorldDefine.h"
#include "VisionModule.h"
#include "staticparams.h"
#include "parammanager.h"
#include "KickStatus.h"
#include "WorldModel.h"
#include "string.h"
#include "math.h"
using namespace std;
namespace {
    bool DEBUG_SWITCH;
    //OUR DATA
    const CGeoPoint OUR_GOAL(-PARAM::Field::PITCH_LENGTH/2,0);
    const CGeoPoint LEFT_GOAL_POST(-PARAM::Field::PITCH_LENGTH/2,-PARAM::Field::GOAL_WIDTH/2 - 2);
    const CGeoPoint RIGHT_GOAL_POST(-PARAM::Field::PITCH_LENGTH/2,PARAM::Field::GOAL_WIDTH/2 + 2);
    const CGeoPoint LEFT_EXCURSION(PARAM::Field::PITCH_LENGTH/2,-PARAM::Field::GOAL_WIDTH/2 - 2 + 150);
    const CGeoPoint RIGHT_EXCURSION(PARAM::Field::PITCH_LENGTH/2,PARAM::Field::GOAL_WIDTH/2 + 2 - 200);
    const CGeoSegment GOAL_LINE(LEFT_GOAL_POST,RIGHT_GOAL_POST);
    const static int MIN_X = -PARAM::Field::PITCH_LENGTH/2;
    const static int MAX_X = -PARAM::Field::PITCH_LENGTH/2 + PARAM::Field::PENALTY_AREA_DEPTH;
    const static int MIN_Y = -PARAM::Field::PENALTY_AREA_WIDTH/2;
    const static int MAX_Y = PARAM::Field::PENALTY_AREA_WIDTH/2;

    //THEIR DATA

    const static int TMIN_X = PARAM::Field::PITCH_LENGTH/2 - PARAM::Field::PENALTY_AREA_DEPTH;
    const static int TMAX_X = PARAM::Field::PITCH_LENGTH/2 + 50;
    const static int TMIN_Y = -PARAM::Field::PENALTY_AREA_WIDTH/2;
    const static int TMAX_Y = PARAM::Field::PENALTY_AREA_WIDTH/2;

    bool in_our_penalty(const CGeoPoint& pos,double padding){
        if(pos.x() > MIN_X + padding && pos.x() < MAX_X - padding && pos.y() > MIN_Y+padding &&pos.y()< MAX_Y -padding)
            return true;
        else
            return false;
    }
    double get_defence_direction(const CGeoPoint & pos){
        double leftPostToBallDir = (pos - LEFT_GOAL_POST).dir();
        double rightPostToBallDir = (pos - RIGHT_GOAL_POST).dir();
        if(DEBUG_SWITCH){
            GDebugEngine::Instance()->gui_debug_line(pos,LEFT_GOAL_POST,6);
            GDebugEngine::Instance()->gui_debug_line(pos,RIGHT_GOAL_POST,6);
        }
        return Utils::Normalize((leftPostToBallDir + rightPostToBallDir) / 2 + PARAM::Math::PI);
    }

    bool in_their_penalty(const CGeoPoint& pos,double padding){
        if(pos.x() > TMIN_X + padding && pos.x() < TMAX_X - padding + PARAM::Field::MAX_PLAYER_SIZE/2 && pos.y() > TMIN_Y + padding && pos.y() < TMAX_Y - padding)
         return true;
        else
         return false;
    }
    bool canshoot(const CGeoPoint &pos,double dir)
    {
        double toupgoaldir  = (RIGHT_GOAL_POST - pos).dir();
        double todowngoaldir= (LEFT_GOAL_POST  - pos).dir();
        if(dir < toupgoaldir && dir > todowngoaldir)
            return true;
        else
            return false;
    }

    bool canflatshoot(const CGeoPoint &origin,const CVisionModule* pVision,const CGeoPoint &target)
    {
        int goalie = -1;
        CGeoLine shootline(target,origin);
        for(int i = 0;i<PARAM::Field::MAX_PLAYER;i++)
        {
             const PlayerVisionT& their = pVision->theirPlayer(i);
             CGeoPoint x = their.Pos();
             if(in_their_penalty(x,0))
             {
                  goalie = i;
             }
        }
        //cout<< goalie<<endl;
        const PlayerVisionT& G = pVision->theirPlayer(goalie);
        CGeoPoint GP = G.Pos();
        CGeoPoint project(shootline.projection(GP));
        cout<<(project - GP).mod()<<endl;
        if(goalie==-1)
        {
            return true;
        }
        if((project - GP).mod() < 120)
            return true;
        else
            return false;
    }

    bool candirectshoot(const double idir,const CVisionModule* pVision,const CGeoPoint &target)
    {
        int goalie = -1;
        CGeoLine shootline(target,idir);
        for(int i = 0;i<PARAM::Field::MAX_PLAYER;i++)
        {
             const PlayerVisionT& their = pVision->theirPlayer(i);
             CGeoPoint x = their.Pos();
             if(in_their_penalty(x,0))
             {
                  goalie = i;
             }
        }
        //cout<< goalie<<endl;
        const PlayerVisionT& G = pVision->theirPlayer(goalie);
        CGeoPoint GP = G.Pos();
        CGeoPoint project(shootline.projection(GP));
        if(goalie==-1)
        {
            return true;
        }
        if((project - GP).mod() < 120)
            return true;
        else
            return false;
    }

    auto player = PARAM::Field::NOW_PLAYER;
    auto maxplayer = PARAM::Field::MAX_PLAYER;
    CGeoPoint excutepos;
    double excutedir;
    int excuteflag;
}

CPenaltyKick::CPenaltyKick():_state(CPenaltyKick::NOTHING)
{
        ZSS::ZParamManager::instance()->loadParam(DEBUG_SWITCH, "DEBUG/Penaltykick", true);
}

void CPenaltyKick::plan(const CVisionModule* pVision){
    if(pVision->getCycle() - _lastCycle > PARAM::Vision::FRAME_RATE*0.1)
    {_state = CPenaltyKick::NOTHING;}
    const int vecNumber = task().executor;
    const PlayerVisionT& self = pVision->ourPlayer(vecNumber);
    TaskT newTask(task());

CGeoPoint ball = pVision ->ball().Pos();
CGeoPoint ipos = self.Pos();
double    idir = self.Dir();
double downstanddir = (ball - LEFT_EXCURSION).dir();
CGeoPoint downstandpos = ball + Utils::Polar2Vector(200,downstanddir);
CGeoPoint standpos = ball + Utils::Polar2Vector(100,downstanddir);
double upstanddir = (ball - RIGHT_EXCURSION).dir();
CGeoPoint upstandpos = ball + Utils::Polar2Vector(90,upstanddir);
CWorldModel p;
int time = p.InfraredOnCount(vecNumber);
//getball
    if(((ipos - ball).mod() > 115 && time<15))
    {
            _state = GETBALL;
            DribbleStatus::Instance()->setDribbleCommand(vecNumber, 3);
            excutepos   = standpos;
            excutedir   = (ball - ipos).dir();
            excuteflag  = PlayerStatus::SLOWLY|PlayerStatus::NOT_AVOID_PENALTY;
    }
    else
    {
        DribbleStatus::Instance()->setDribbleCommand(vecNumber, 3);
        if (canflatshoot(ball,pVision,LEFT_EXCURSION))
        {

            if(canshoot(ipos,idir))
            {
                _state = SHOOT;
                KickStatus::Instance()->setKick(vecNumber,6500);
                excutepos   = standpos;
                excutedir   = (LEFT_EXCURSION - ipos).dir();
                excuteflag  = PlayerStatus::NOT_AVOID_PENALTY;
            }
            else
            {
                 _state = DECEPTIVE;
                DribbleStatus::Instance()->setDribbleOff(vecNumber);
                excutepos   = standpos;
                excutedir   = (LEFT_EXCURSION - ipos).dir();
                excuteflag  = PlayerStatus::NOT_AVOID_PENALTY;
            }
        }
        else
        {
            if(candirectshoot(idir,pVision,ball))
            {
                _state = SHOOT;
                cout<<6<<endl;
                KickStatus::Instance()->setKick(vecNumber,6500);
                DribbleStatus::Instance()->setDribbleOff(vecNumber);
                excutepos   = ipos;
                excutedir   = (RIGHT_EXCURSION - ipos).dir();
                excuteflag  = PlayerStatus::NOT_AVOID_PENALTY;
            }
            else
            {
                 _state = DECEPTIVE;
                excutepos   = ipos;
                excutedir   = (RIGHT_EXCURSION - ipos).dir();
                excuteflag  = PlayerStatus::NOT_AVOID_PENALTY;
            }
        }
    }
cout<<candirectshoot(idir,pVision,ball)<<endl;
    if(DEBUG_SWITCH){
        switch(_state){
        case NOTHING:
            debug_state = "NOTHING";
            break;
        case GETBALL:
            debug_state = "GETBALL";
            break;
        case DECEPTIVE:
            debug_state = "DECEPTIVE";
            break;
        case SHOOT:
            debug_state = "SHOOT";
            break;
        default:
            debug_state = "ERROR STATE";
        }
        GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(-PARAM::Field::PITCH_LENGTH/2,PARAM::Field::GOAL_WIDTH/2),debug_state.c_str());
    }
      newTask.player.pos   = excutepos;
      newTask.player.angle = excutedir;
      newTask.player.flag  = excuteflag;

      setSubTask(TaskFactoryV2::Instance()->SmartGotoPosition(newTask));
    //setSubTask(PlayerRole::makeItSmartGoto(vecNumber,CGeoPoint(100,100),0,0));
    _lastCycle = pVision->getCycle();
    CPlayerTask::plan(pVision);
}
